// A simple demo of Gauss-Newton algorithm on a user defined function

#include <cstdio>
#include <vector>
#include <opencv2/core/core.hpp>
#include <math.h>
#include "Segment.h";

#include "G1SegmentsFitting.h";
using namespace std;
using namespace cv;


int main()
{
	vector<Segment*> segments;

	CircleArc arc1 = CircleArc();
	arc1.startingPoint = Point2d(0.99451744, 0.068590453);
	arc1.startingAngle = 1.605702912;
	arc1.endingAngle = 3.176499239;
	arc1.radius = 0.9;

	StraightLine line1 = StraightLine();
	line1.startingPoint = Point2d(-0.1, 1);
	line1.startingAngle = PI + 0.03;
	line1.length = 0.98;

	CircleArc arc2 = CircleArc();
	arc2.startingPoint = Point2d(-1.03, 1.0);
	arc2.startingAngle = 3.17;
	arc2.endingAngle = 1.59;
	arc2.radius = 1;

	CircleArc arc3 = CircleArc();
	arc3.startingPoint = Point2d(-2.03, 2.0);
	arc3.startingAngle = PI/2+0.02;
	arc3.endingAngle = PI-0.03;
	arc3.radius = 1;

		
	//CircleArc arc1 = CircleArc();
	//arc1.startingPoint = Point2d(0.9, -0.1);
	//arc1.startingAngle = PI/2;
	//arc1.endingAngle = PI;
	//arc1.radius = 1;

	//StraightLine line1 = StraightLine();
	//line1.startingPoint = Point2d(-0.1,0.92);
	//line1.startingAngle = PI;
	//line1.length = 1;

	segments.push_back(&arc1);
	segments.push_back(&line1);
	segments.push_back(&arc2);
	segments.push_back(&arc3);


	//compute observedData according to segments
	int numberOfNeededPoints = 0;
	for(int i = 0;i<segments.size();i++)
	{
		if (segments.at(i)->type == SEGTYPE_LINE) numberOfNeededPoints+=2;
		else if (segments.at(i)->type == SEGTYPE_CIRC) numberOfNeededPoints+=3;
	}

	Mat observedData(numberOfNeededPoints*2, 1, CV_64F);
	for(int i =0, inputIndex =0; i<segments.size();i++)
	{
		if (segments.at(i)->type == SEGTYPE_LINE)
		{
			StraightLine line = * (dynamic_cast<StraightLine *>(segments.at(i)));
			observedData.at<double>(inputIndex, 0) = line.startingPoint.x;
			inputIndex++;

			observedData.at<double>(inputIndex, 0) = line.startingPoint.y;
			inputIndex++;

			observedData.at<double>(inputIndex, 0) = line.startingPoint.x + line.length * cos(line.startingAngle); 
			inputIndex++;

			observedData.at<double>(inputIndex, 0) = line.startingPoint.y + line.length * sin(line.startingAngle); 
			inputIndex++;

		}
		else if(segments.at(i)->type == SEGTYPE_CIRC)
		{
			CircleArc arc = * (dynamic_cast<CircleArc *>(segments.at(i)));
			double sign = 1;
			if(arc.endingAngle < arc.startingAngle) sign = -1;

			double centerX = arc.startingPoint.x - arc.radius*cos(arc.startingAngle - sign*PI/2);
			observedData.at<double>(inputIndex, 0) = centerX;
			inputIndex++;

			double centerY = arc.startingPoint.y - arc.radius*sin(arc.startingAngle - sign*PI/2);
			observedData.at<double>(inputIndex, 0) = centerY;
			inputIndex++;

			observedData.at<double>(inputIndex, 0) = arc.startingPoint.x;
			inputIndex++;

			observedData.at<double>(inputIndex, 0) = arc.startingPoint.y;
			inputIndex++;

			observedData.at<double>(inputIndex, 0) = centerX +  arc.radius*cos(arc.endingAngle - sign*PI/2);
			inputIndex++;

			observedData.at<double>(inputIndex, 0) = centerY+  arc.radius*sin(arc.endingAngle - sign*PI/2);
			inputIndex++;
		}
	}
	printMatrix(observedData);

	printSegments(segments, string("D:\\AixMarseille\\GeneratedModels\\Orginal.obj"), "Orignal");

    //GaussNewton(observedData, segments);
	//LevenbergMarquardt(observedData, segments, 0.5, 2,2);
	
	G1SegmentsFitting al = G1SegmentsFitting(observedData, segments);
	Mat params = getParamsFromSegments(segments);
	al.solveByGaussNewton(params);

	Mat fullParams = convertToFullParams(params, segments);
	for(int i = 0 ;i<fullParams.rows;i++)
	{
		segments.at(i)->startingPoint.x = fullParams.at<double>(i,0);
		segments.at(i)->startingPoint.y = fullParams.at<double>(i,1);
		segments.at(i)->startingAngle = fullParams.at<double>(i,2);

		if (segments.at(i)->type == SEGTYPE_LINE)
		{
			dynamic_cast<StraightLine *>(segments.at(i))->length = fullParams.at<double>(i,3);
		}
		else if(segments.at(i)->type == SEGTYPE_CIRC)
		{
			CircleArc* arc = dynamic_cast<CircleArc *>(segments.at(i));
			arc->endingAngle =  fullParams.at<double>(i,3);
			arc->radius =  fullParams.at<double>(i,4);
		}
	}

	//printSegments(segments, string("D:\\AixMarseille\\GeneratedModels\\FittingGaussNewton.obj"), "GaussNewton");
	printSegments(segments, string("D:\\AixMarseille\\GeneratedModels\\FittingLevenbergMarquardt.obj"), "LevenbergMarquardt");

	

	system("PAUSE");
    return 0;
}
